/******************************************************************************
**
** Copyright 2011 Motorola Solutions Inc. All rights reserved.
**
** File name: 	uhwinit.c
**
** Description:	Basic hardware initialization required for RTOS Unit
**				Tests execution. Targets supported:
**				- ATMega128   (SIM)
**				- ATXmega32D4 (HW)
**
** Change history:
**	
** 07/10/2011 - AK - File created. 
** 10/10/2011 - AK - Timer control services are separated from initialization
** 11/20/2011 - AK - Two timers support, removed support for AVRSIM
** 
******************************************************************************/
#include <avr/io.h>
#include <avr/interrupt.h>
#include "uhwinit.h"


/* Forward function declarations */
void CCPWrite( volatile uint8_t * address, uint8_t value );

#if defined( __AVR_ATxmega32D4__ )

/******************************************************************************
** FUNCTION:	utst_init_gpio( void )
** DESCRIPTION:	No port initialization is required now.	All GPIO pins are 
**				in its default (after reset) state.
** PASSED:      void
** RETURNS:		void
** USES:		-
** STACK:		-
** CALLS:		-
** ASSUMES:		-
** NOTES:		Will get back to this service if external crystal is used.
**				Probably will use some GPIO as debug pin out.
******************************************************************************/
void utst_init_gpio( void )
{
	
	/******************************************************************************
	** -= PORTD =-
	**	Pin    Description     Direction  Pullup  Invert   Sense    Default
	**	-------------------------------------------------------------------
	**	PIN3 - Debug UART TX     Out        No      No       NA       High
	******************************************************************************/ 
	//PORTCFG.MPCMASK = PIN3_bm;
	//PORTD.PIN0CTRL = PORT_OPC_PULLUP_gc | PORT_ISC_INPUT_DISABLE_gc;
	
	// set direction 
	PORTD_DIR = PIN3_bm;
	PORTD_OUT = PIN3_bm;
	
	PORTCFG.MPCMASK = 0;

	return;
}

/******************************************************************************
** FUNCTION:	void utst_init_clksys( void )
** DESCRIPTION:	Configure the system clock to use internal 2MHz oscillator.
**				Internal 32KHz oscillator is used for calibration.
** PASSED:      void
** RETURNS:		void
** USES:		-
** STACK:		-
** CALLS:		-
** ASSUMES:		-
** NOTES:		-
******************************************************************************/
void utst_init_clksys( void )
{
	uint8_t tmp;
    //  Enable the internal 32 kHz oscillator.
    //  CLKSYS_Enable(OSC_RC32KEN_bm);
	OSC.CTRL |= OSC_RC32KEN_bm;
    
    //  Enable the internal 32 MHz oscillator.  This is the default oscillator
    //  so it should already be up, but we'll select it anyway to make sure.
    // 	CLKSYS_Enable(OSC_RC2MEN_bm);
	OSC.CTRL |= OSC_RC32MEN_bm;
    
    //  Wait for the 32 kHz clock to come up.  Used to increase the
    //  accuracy of the 2 MHz internal clock using the DFLL.
    //  do {} while (CLKSYS_IsReady(OSC_RC32KEN_bm) == 0);
	do {} while ( 0 == (OSC.STATUS & OSC_RC32KEN_bm) );

    //  Wait for the 32 MHz clock to come up.
    //  do {} while (CLKSYS_IsReady(OSC_RC2MEN_bm) == 0);
	do {} while ( 0 == (OSC.STATUS & OSC_RC32MEN_bm) );
    
    //  Enable the auto calibration feature on the 32 MHz clock using the
    //  internal 32 kHz oscillator as a reference.
    // CLKSYS_AutoCalibration_Enable(OSC_RC2MCREF_bm, FALSE);
	OSC.DFLLCTRL = 0; // select internal 32kHz osc as a calibration source for both 2MHz and 32Mhz osc's
	DFLLRC2M.CTRL |= DFLL_ENABLE_bm;
    
    //  Set the pre-scalers to divide by 4
    //  CLKSYS_Prescalers_Config(CLK_PSADIV_4_gc, CLK_PSBCDIV_1_1_gc);
	CCPWrite( &CLK.PSCTRL, (CLK_PSADIV_4_gc | CLK_PSBCDIV_1_1_gc) );

    //  Select the internal 32 MHz clock as the system clock.  Program can
    //  hang here if clock is not ready.
    //  do {} while (CLKSYS_Main_ClockSource_Select(CLK_SCLKSEL_RC2M_gc) !=
    //                                                    CLK_SCLKSEL_RC2M_gc);
	do {
		tmp = (( CLK.CTRL & ~CLK_SCLKSEL_gm ) | CLK_SCLKSEL_RC32M_gc);
		CCPWrite( &CLK.CTRL, tmp );
	} while ( CLK_SCLKSEL_RC32M_gc != ( CLK.CTRL & CLK_SCLKSEL_RC32M_gc ) );
	
	
    // Disable the ATXmega external oscillator interface.
	// CLKSYS_Disable(OSC_XOSCEN_bm);
	OSC.CTRL &= ~OSC_XOSCEN_bm;


    //  De-assert the enable pin on the external 12 MHz oscillator.
    // PORT_ClearOutputBit(&PORTC, PIN4_bp);
	PORTC.OUT = PORTC.OUT & ~(1 << PIN4_bp);
	

	// Disable the internal 2Mhz oscillator
	// CLKSYS_Disable(OSC_RC2MEN_bm);
	OSC.CTRL &= ~OSC_RC2MEN_bm;


    // Ensure PLL is disabled.
    // CLKSYS_Disable(OSC_PLLEN_bm);
	OSC.CTRL &= ~OSC_PLLEN_bm;

    // Ensure RTC is disabled.
    // CLKSYS_RTC_ClockSource_Disable();
	CLK.RTCCTRL &= ~CLK_RTCEN_bm;
}

/******************************************************************************
** FUNCTION:	void utst_init_pmic( void )
** DESCRIPTION:	Configures Xmega interrupt controller.
** PASSED:      void
** RETURNS:		void
** USES:		-
** STACK:		-
** CALLS:		-
** ASSUMES:		-
** NOTES:		-
******************************************************************************/
void utst_init_pmic( void )
{
	// Set vector location to application
	uint8_t temp = PMIC.CTRL & ~PMIC_IVSEL_bm;
	CCP = CCP_IOREG_gc;
	PMIC.CTRL = temp;
	
	//PMIC.CTRL &= ~PMIC_RREN_bm;	// Disable round-robin
	PMIC_CTRL |= PMIC_RREN_bm;		// Enable round-robin for LOW level interrupts
	
	// Enable all low/medium/high levels interrupts
	PMIC.CTRL |= (PMIC_LOLVLEN_bm | PMIC_MEDLVLEN_bm | PMIC_HILVLEN_bm);
}

#endif	/* __AVR_ATxmega32D4__ */


/******************************************************************************
** FUNCTION:	void utst_init_usart( void )
** DESCRIPTION: Initialize USART_D module to operate as RTOS debug port.
**				
** PASSED:      None
** RETURNS:		None
** USES:		-
** STACK:		-
** CALLS:		-
** ASSUMES:		Xmega MCU clock speed is 8MHz;
**				GPIO PORT D pin 3 is configured prior to call this service.
** NOTES:		-
******************************************************************************/
void utst_init_usart( void )
{
	// USART set format
	USARTD0.CTRLC = (uint8_t) (USART_CHSIZE_8BIT_gc | USART_PMODE_DISABLED_gc | USART_SBMODE_bm);
	// USART set baudrate
	USARTD0.BAUDCTRLA =(uint8_t)214;                                           
	USARTD0.BAUDCTRLB =(0x0A << USART_BSCALE0_bp)|(214 >> 8);
	// Enable Tx
	USARTD0.CTRLB |= USART_TXEN_bm;

}

/******************************************************************************
** FUNCTION:	void CCPWrite( volatile uint8_t * address, uint8_t value )
** DESCRIPTION: CCP write helper function written in assembly.		
** PASSED:      address - A pointer to the address to write to.
** 				value   - The value to put in to the register.
** RETURNS:		void
** USES:		-
** STACK:		-
** CALLS:		-
** ASSUMES:		-
** NOTES:		-
******************************************************************************/
void CCPWrite( volatile uint8_t * address, uint8_t value )
{
	volatile uint8_t * tmpAddr = address;
#ifdef RAMPZ
	RAMPZ = 0;
#endif
	asm volatile(
		"movw r30,  %0"	      "\n\t"
		"ldi  r16,  %2"	      "\n\t"
		"out   %3, r16"	      "\n\t"
		"st     Z,  %1"       "\n\t"
		:
		: "r" (tmpAddr), "r" (value), "M" (CCP_IOREG_gc), "i" (&CCP)
		: "r16", "r30", "r31"
		);
}